#include "protocol/modbus/ModbusTCP.h"
#include "core/Logger.h"

#include <modbus.h>
#include <cerrno>
#include <cstring>

namespace ems {
namespace protocol {

ModbusTCP::ModbusTCP(const std::string& host, int port)
    : host_(host), port_(port), slaveId_(1), timeout_(3000), ctx_(nullptr), connected_(false) {
}

ModbusTCP::~ModbusTCP() {
    disconnect();
}

bool ModbusTCP::connect() {
    if (connected_) {
        return true;
    }

    if (ctx_) {
        modbus_free(ctx_);
        ctx_ = nullptr;
    }

    ctx_ = modbus_new_tcp(host_.c_str(), port_);
    if (!ctx_) {
        int err = errno;
        updateLastError(err);
        LOG_ERROR("Failed to create Modbus TCP context: " << lastError_);
        return false;
    }

    if (modbus_set_slave(ctx_, slaveId_) == -1) {
        int err = errno;
        updateLastError(err);
        LOG_ERROR("Failed to set Modbus slave ID: " << lastError_);
        modbus_free(ctx_);
        ctx_ = nullptr;
        return false;
    }
    
    struct timeval timeout;
    timeout.tv_sec = timeout_ / 1000;
    timeout.tv_usec = (timeout_ % 1000) * 1000;

    if (modbus_set_response_timeout(ctx_, timeout.tv_sec, timeout.tv_usec) == -1) {
        int err = errno;
        updateLastError(err);
        LOG_WARN("Failed to set response timeout: " << lastError_);
    }

    if (modbus_connect(ctx_) == -1) {
        int err = errno;
        updateLastError(err);
        LOG_ERROR("Failed to connect to Modbus TCP server: " << lastError_);
        modbus_free(ctx_);
        ctx_ = nullptr;
        return false;
    }
    
    connected_ = true;
    LOG_INFO("Successfully connected to Modbus TCP server: " << host_ << ":" << port_);
    return true;
}

void ModbusTCP::disconnect() {
    if (ctx_ || connected_) {
        markDisconnected();
        LOG_INFO("Disconnected from Modbus TCP server");
    }
}

bool ModbusTCP::isConnected() const {
    return connected_ && ctx_ != nullptr;
}

void ModbusTCP::setSlaveId(int slaveId) {
    slaveId_ = slaveId;
    if (ctx_) {
        modbus_set_slave(ctx_, slaveId_);
    }
}

void ModbusTCP::setTimeout(int timeoutMs) {
    timeout_ = timeoutMs;
    if (ctx_) {
        struct timeval timeout;
        timeout.tv_sec = timeoutMs / 1000;
        timeout.tv_usec = (timeoutMs % 1000) * 1000;
        modbus_set_response_timeout(ctx_, timeout.tv_sec, timeout.tv_usec);
    }
}

bool ModbusTCP::readHoldingRegisters(uint16_t address, uint16_t count, std::vector<uint16_t>& data) {
    if (!ensureConnection()) {
        lastError_ = "Not connected";
        return false;
    }

    data.resize(count);
    int result = modbus_read_registers(ctx_, address, count, data.data());
    if (result == -1) {
        int err = errno;
        updateLastError(err);
        handleConnectionFailure(err, "read holding registers");
        LOG_ERROR("Failed to read holding registers: " << lastError_);
        return false;
    }

    return true;
}

bool ModbusTCP::readInputRegisters(uint16_t address, uint16_t count, std::vector<uint16_t>& data) {
    if (!ensureConnection()) {
        lastError_ = "Not connected";
        return false;
    }

    data.resize(count);
    int result = modbus_read_input_registers(ctx_, address, count, data.data());
    if (result == -1) {
        int err = errno;
        updateLastError(err);
        handleConnectionFailure(err, "read input registers");
        LOG_ERROR("Failed to read input registers: " << lastError_);
        return false;
    }
    
    return true;
}

bool ModbusTCP::readCoils(uint16_t address, uint16_t count, std::vector<bool>& data) {
    if (!ensureConnection()) {
        lastError_ = "Not connected";
        return false;
    }

    std::vector<uint8_t> rawData(count);
    int result = modbus_read_bits(ctx_, address, count, rawData.data());
    if (result == -1) {
        int err = errno;
        updateLastError(err);
        handleConnectionFailure(err, "read coils");
        LOG_ERROR("Failed to read coils: " << lastError_);
        return false;
    }
    
    data.resize(count);
    for (size_t i = 0; i < count; ++i) {
        data[i] = rawData[i] != 0;
    }
    
    return true;
}

bool ModbusTCP::readDiscreteInputs(uint16_t address, uint16_t count, std::vector<bool>& data) {
    if (!ensureConnection()) {
        lastError_ = "Not connected";
        return false;
    }

    std::vector<uint8_t> rawData(count);
    int result = modbus_read_input_bits(ctx_, address, count, rawData.data());
    if (result == -1) {
        int err = errno;
        updateLastError(err);
        handleConnectionFailure(err, "read discrete inputs");
        LOG_ERROR("Failed to read discrete inputs: " << lastError_);
        return false;
    }
    
    data.resize(count);
    for (size_t i = 0; i < count; ++i) {
        data[i] = rawData[i] != 0;
    }
    
    return true;
}

bool ModbusTCP::writeSingleRegister(uint16_t address, uint16_t value) {
    if (!ensureConnection()) {
        lastError_ = "Not connected";
        return false;
    }

    int result = modbus_write_register(ctx_, address, value);
    if (result == -1) {
        int err = errno;
        updateLastError(err);
        handleConnectionFailure(err, "write single register");
        LOG_ERROR("Failed to write single register: " << lastError_);
        return false;
    }

    return true;
}

bool ModbusTCP::writeMultipleRegisters(uint16_t address, const std::vector<uint16_t>& values) {
    if (!ensureConnection()) {
        lastError_ = "Not connected";
        return false;
    }

    int result = modbus_write_registers(ctx_, address, values.size(), values.data());
    if (result == -1) {
        int err = errno;
        updateLastError(err);
        handleConnectionFailure(err, "write multiple registers");
        LOG_ERROR("Failed to write multiple registers: " << lastError_);
        return false;
    }

    return true;
}

bool ModbusTCP::writeSingleCoil(uint16_t address, bool value) {
    if (!ensureConnection()) {
        lastError_ = "Not connected";
        return false;
    }

    int result = modbus_write_bit(ctx_, address, value ? 1 : 0);
    if (result == -1) {
        int err = errno;
        updateLastError(err);
        handleConnectionFailure(err, "write single coil");
        LOG_ERROR("Failed to write single coil: " << lastError_);
        return false;
    }
    
    return true;
}

bool ModbusTCP::writeMultipleCoils(uint16_t address, const std::vector<bool>& values) {
    if (!ensureConnection()) {
        lastError_ = "Not connected";
        return false;
    }

    std::vector<uint8_t> rawData(values.size());
    for (size_t i = 0; i < values.size(); ++i) {
        rawData[i] = values[i] ? 1 : 0;
    }

    int result = modbus_write_bits(ctx_, address, values.size(), rawData.data());
    if (result == -1) {
        int err = errno;
        updateLastError(err);
        handleConnectionFailure(err, "write multiple coils");
        LOG_ERROR("Failed to write multiple coils: " << lastError_);
        return false;
    }
    
    return true;
}

std::string ModbusTCP::getLastError() const {
    return lastError_;
}

bool ModbusTCP::ensureConnection() {
    if (isConnected()) {
        return true;
    }

    LOG_INFO("Modbus TCP attempting to connect to " << host_ << ":" << port_);
    return connect();
}

void ModbusTCP::handleConnectionFailure(int errCode, const std::string& action) {
    if (!isConnectionError(errCode)) {
        return;
    }

    LOG_WARN("Modbus TCP connection lost during " << action << ": " << lastError_);
    markDisconnected();
}

void ModbusTCP::markDisconnected() {
    if (ctx_) {
        modbus_close(ctx_);
        modbus_free(ctx_);
        ctx_ = nullptr;
    }
    connected_ = false;
}

bool ModbusTCP::isConnectionError(int errCode) const {
    switch (errCode) {
        case ECONNRESET:
        case ECONNABORTED:
        case ECONNREFUSED:
        case ENOTCONN:
        case EPIPE:
        case ETIMEDOUT:
#ifdef EHOSTUNREACH
        case EHOSTUNREACH:
#endif
#ifdef ENETDOWN
        case ENETDOWN:
#endif
#ifdef ENETRESET
        case ENETRESET:
#endif
#ifdef ENETUNREACH
        case ENETUNREACH:
#endif
            return true;
        default:
            return false;
    }
}

void ModbusTCP::updateLastError(int errCode) const {
    if (errCode == 0) {
        lastError_.clear();
    } else {
        lastError_ = std::string(modbus_strerror(errCode));
    }
}

}  // namespace protocol
}  // namespace ems
